﻿using System;
using System.Net;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Ink;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Shapes;
using System.Collections.Generic;
using System.Diagnostics;

namespace ParticleFiltersDemo1
{
    /*
     * Used to control the robot, move the particles and define the sampling steps.
     */ 
    public class Controller : ViewModelBase
    {
        private int particlesCount = 200;
        private Robot robot;
        private Board board;
        private ParticlesSet particleSet;

        public Controller()
        {
            this.board = new Board();
            this.robot = new Robot( 3, 4, 90, false, this.board );

            ParticlesSet intialSet = new ParticlesSet( particlesCount, this.board );
            intialSet.InitializeParticles();
            this.particleSet = intialSet;
        }

        public Robot Robot
        {
            get
            {
                return this.robot;
            }
            set
            {
                if ( this.robot == value )
                    return;

                this.robot = value;
                this.NotifyPropertyChanged( "Robot" );
            }
        }

        public ParticlesSet Particles
        {
            get
            {
                return this.particleSet;
            }
            set
            {
                if ( this.particleSet == value )
                    return;

                this.particleSet = value;
                this.NotifyPropertyChanged( "Particles" );
            }
        }

        /*
         * The core part of the algorithm is implemented here.
         */ 
        public bool DoFiltering()
        {
            if ( Particles.IsDone )
            {
                MessageBox.Show( "I already know where I am!" );
                return true;
            }

            Random random = new Random();

            // Move the robot
            robot.Move();

            // Move the particles
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                this.Particles.Particles[i].SetHeading( robot.Heading );
            }

            ParticlesSet newSet = new ParticlesSet( this.particlesCount, this.board );
            Particle currentParticle;
            List<int> probabilityPoints = new List<int>();

            //Constructing helper array of all particles many times for helping choosing ones
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                double particleWeight = this.Particles.Particles[i].weight;

                int particleRatio = ( int )( particleWeight * 1000 );

                for ( int j = 0; j < particleRatio; j++ )
                    probabilityPoints.Add( i );
            }

            if ( probabilityPoints.Count == 0 )
                return false;

            int particlesNextToOpsticle = 0;

            //Fething particles with replasment and counting particlesNextToOpsticle 
            for ( int i = 0; i < this.particlesCount; i++ )
            {
                int index = probabilityPoints[random.Next( 0, probabilityPoints.Count - 1 )];
                currentParticle = this.Particles.Particles[index];
                newSet.Particles[i] = currentParticle.Clone();

                if ( newSet.Particles[i].AdvanceBy( this.robot.speed, this.robot.noisy ) == false )
                {
                    newSet.Particles[i].weight = 0;
                }

                if ( newSet.Particles[i].IsNextToObstacle )
                {
                    particlesNextToOpsticle++;
                }
            }

            if ( particlesNextToOpsticle == 0 )
                particlesNextToOpsticle = 1;

            if ( particlesCount == particlesNextToOpsticle )
                return false;

            double weightsSum = 0;

            //Calculating new weight of particles 
            if ( robot.IsNextToObstacle )
            {
                for ( int i = 0; i < this.particlesCount; i++ )
                {
                    if ( newSet.Particles[i].weight == 0 )
                    {
                        continue;
                    }
                    if ( newSet.Particles[i].IsNextToObstacle )
                    {
                        newSet.Particles[i].weight = 0.8 / particlesNextToOpsticle;
                    }
                    else
                    {
                        newSet.Particles[i].weight = 0.2 / ( particlesCount - particlesNextToOpsticle );
                    }

                    weightsSum += newSet.Particles[i].weight;
                }
            }
            else
            {
                for ( int i = 0; i < this.particlesCount; i++ )
                {
                    if ( newSet.Particles[i].weight == 0 )
                    {
                        continue;
                    }
                    if ( !newSet.Particles[i].IsNextToObstacle )
                    {
                        newSet.Particles[i].weight = 0.8 / particlesNextToOpsticle;
                    }
                    else
                    {
                        newSet.Particles[i].weight = 0.2 / ( particlesCount - particlesNextToOpsticle );
                    }

                    weightsSum += newSet.Particles[i].weight;
                }
            }

            if ( weightsSum == 0 )
                return false;

            for ( int i = 0; i < this.particlesCount; i++ )
            {
                newSet.Particles[i].weight = newSet.Particles[i].weight / weightsSum;
            }
            
            this.Particles = newSet;

            // Calculate particles weight
            // Calculate meanpoint
            this.particleSet.ComputeMeanPoint( this.board );

            return false;
        }
    }
}
